/*
 * Copyright (c) 2020 - ~, HIT_HERO Team
 *
 * PCCOMTEST APP SOUCE FILE
 * Used in RT-Thread Operate System
 *
 * Change Logs:
 * Date           Author       Notes                Mail
 * 2020-09-06     WangXi       first version        WangXi_chn@foxmail.com
 */
 
#include "App_Sensor.h"
#include <stdio.h>

static void APP_SensorInit(APP_SENSOR *Application);
static void APP_SensorRun(APP_SENSOR *Application);

static void SensorLed_entry(void *parameter);
static void SensorUpdate_entry(void *parameter);
static void SensorDeal_entry(void *parameter);
static void SensorControl_entry(void *parameter);

void APP_Sensor_Config(APP_SENSOR *Application)
{
	if(	Application->Method_Init	==	NULL &&
		Application->Method_Run		==	NULL

    ){  
        /* Link the Method */
        Application->Method_Init = APP_SensorInit;
		Application->Method_Run	 = APP_SensorRun;
		
    }
    else{
        rt_kprintf("Warning: Module Led is Configed twice\n");
        return;
    }

    /* Device Init */
    Application->Method_Init(Application);
	
	/* Module Config */
	Module_UartCom_Config(&(Application->dev_UartBsp));
	Module_Led_Config(&(Application->dev_Led));
	Module_Oled_Config(&(Application->dev_Oled));
	Module_DjiC610620Group_Config(&(Application->dev_Motorgroup));
	
    return;
}

static void APP_SensorInit(APP_SENSOR *Application)
{
	/* Module param list ------------------------------------------------------------------------- */
	/* LED Pin device */
	/* Pin:	PC13 Low power enable */
	Application->dev_Led.Property_pin = GET_PIN(C, 13);
	Application->dev_Led.Property_Mode = FLASH_LED_MODE;
	Application->dev_Led.LED_TIME_CYCLE = 1500;
	Application->dev_Led.LED_TIME_OUTPUT = 150;

	/* JC24B 2.4G Uart communication device */
	/* Pin:	PA2 Uart2_TX */
	/* Pin:	PA3 Uart2_RX */
	/* Pin:	PA4 Set Pin */
	Application->dev_UartBsp.Property_UartDevName   = "uart2";
	Application->dev_UartBsp.Property_BaudRate		= 9600;
	Application->dev_UartBsp.Property_MyID			= 0x02;
	Application->dev_UartBsp.Property_NetID			= 0x10;
	Application->dev_UartBsp.Property_Device		= JC24B;
	Application->dev_UartBsp.Property_JC24BsetPin	= GET_PIN(A, 4);
	
	/* OLED communication device */
	/* Pin:	PA6 OLED SCK */
	/* Pin:	PA5 OLED SDA */
	Application->dev_Oled.Property_SCKpin = GET_PIN(A, 6);
	Application->dev_Oled.Property_SDApin = GET_PIN(A, 5);
	Application->dev_Oled.Property_Y_WIDTH = 32;
	Application->dev_Oled.Property_X_WIDTH = 128;

	Application->dev_Motorgroup.Property_CanDevName = "can1";
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].Enable = 1;
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].Mode = DjiC610620MODE_SPEED;
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].ENCODER = DjiC610620SELF;
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].PID_Speed.Property_Kp = 3.5;
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].PID_Speed.Property_Ki = 1;
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].PID_Speed.Property_Kd = 0;
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].PID_Speed.Property_dt = 0.005;
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].PID_Speed.Property_integralMax = 500;
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].PID_Speed.Property_AimMax = 14000;
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].PID_Speed.Property_OutputMax = 10000;
	Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].PID_Speed.Property_integralErrMax = 500;
	/* Data Sheet Default Value ------------------------------------------------------------------ */
	
	
}

static void APP_SensorRun(APP_SENSOR *Application)
{
	/* Let the LED flash module show the machine ID */	
	Application->dev_Led.Method_Set(&Application->dev_Led,Application->dev_UartBsp.Property_MyID);
	char ch[20];
	sprintf(ch,"TEST MACHAIN");
	Application->dev_Oled.Method_ShowStr(&(Application->dev_Oled),0,0,ch,2);
	sprintf(ch,"ID: %d              ",Application->dev_UartBsp.Property_MyID);
	ch[14]='\0';
	Application->dev_Oled.Method_ShowStr(&(Application->dev_Oled),0,2,ch,2);
	
	
	rt_thread_t SensorLed_thread = rt_thread_create("SensorLed", SensorLed_entry, 
									Application,512, RT_THREAD_PRIORITY_MAX - 2, 20);
    if (SensorLed_thread != RT_NULL){
        rt_thread_startup(SensorLed_thread);
    }
	
	rt_thread_t SensorUpdate_thread = rt_thread_create("SensorUpdate", SensorUpdate_entry, 
									Application,512, RT_THREAD_PRIORITY_MAX - 4, 20);
    if (SensorUpdate_thread != RT_NULL){
        rt_thread_startup(SensorUpdate_thread);
    }	
	
	rt_thread_t SensorDeal_thread = rt_thread_create("SensorDeal", SensorDeal_entry, 
									Application,1024, RT_THREAD_PRIORITY_MAX - 3, 20);
    if (SensorDeal_thread != RT_NULL){
        rt_thread_startup(SensorDeal_thread);
    }
	
	rt_thread_t SensorControl_thread = rt_thread_create("SensorControl", SensorControl_entry, 
									Application,512, RT_THREAD_PRIORITY_MAX - 4, 20);
    if (SensorControl_thread != RT_NULL){
        rt_thread_startup(SensorControl_thread);
    }
}


/* thread entry list ----------------------------------------------------------------------------- */
static void SensorLed_entry(void *parameter)
{
	APP_SENSOR *Application = (APP_SENSOR *)parameter;
	while(1)
	{
		rt_thread_mdelay(1);
		Application->dev_Led.Method_Handle(&(Application->dev_Led));
	}	
}

static void SensorUpdate_entry(void *parameter)
{
	APP_SENSOR *Application = (APP_SENSOR *)parameter;

	Application->dev_UartBsp.Method_Feed(	&(Application->dev_UartBsp),
											&(Application->Value_PacketID),
											Application->Value_DataSheet);	
}

static void SensorDeal_entry(void *parameter)
{
	APP_SENSOR *Application = (APP_SENSOR *)parameter;
	char ch[50];
	rt_uint16_t count = 0;
		
    while (1)
	{
		rt_thread_mdelay(10);
		
		
		/* Input the data to motor controler and get output */
		Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].Value_motor_AimAngle = Application->Value_DataSheet[coactualangle];
		Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].Value_motor_AimRPM = Application->Value_DataSheet[cogoalspinspeed] * 5;
		
		Application->Value_DataSheet[coactualangle] = Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].Value_motor_TotalAngle;
		Application->Value_DataSheet[coactualspinspeed] = Application->dev_Motorgroup.Value_module_DjiC610620[DjiC610620ID_1].Value_motor_RealRPM;
		

		/* Send the controler output data to console */
//		Application->dev_UartBsp.Value_AimID = 0x02;
//		Application->Value_DataSheet[coactualangle] = count;
//		Application->Value_DataSheet[coactualspinspeed] = count;
//		Application->dev_UartBsp.Method_Send(&(Application->dev_UartBsp),coactualangle,Application->Value_DataSheet[coactualangle]);
//		Application->dev_UartBsp.Method_Send(&(Application->dev_UartBsp),coactualspinspeed,Application->Value_DataSheet[coactualspinspeed]);
//		
		
		/* Show the controler input data on OLED */
		count++;
		if(count == 10)
		{
			count = 0;
						
			sprintf(ch,"Ar:%d Aa:%d      ",Application->Value_DataSheet[coactualangle],Application->Value_DataSheet[cogoalangle]);
			ch[14]='\0';
			Application->dev_Oled.Method_ShowStr(&(Application->dev_Oled),0,4,ch,2);
			
			sprintf(ch,"Sr:%d Sa:%d      ",Application->Value_DataSheet[coactualspinspeed],Application->Value_DataSheet[cogoalspinspeed]);
			ch[14]='\0';
			Application->dev_Oled.Method_ShowStr(&(Application->dev_Oled),0,6,ch,2);
		}	
    }
}

static void SensorControl_entry(void *parameter)
{
	APP_SENSOR *Application = (APP_SENSOR *)parameter;
	while (1)
    {   
        /* Communicate with motor group */      
        Application->dev_Motorgroup.Method_Feed(&(Application->dev_Motorgroup));
        rt_thread_mdelay(1);
        Application->dev_Motorgroup.Method_Send(&(Application->dev_Motorgroup));
    }
}

/************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/
